MecanumDriveOdometry to handle odometry estimation of Mecanum wheeled models #486

danilogsch · 2022-08-25T16:01:33Z

🎉 New feature

Closes #683

Summary

Math Class to handle odometry estimation of Mecanum wheeled models.
Observation: Aproximation when the robot is given both linear, lateral and angular velocity commands. Exact pose integration or Runge Kuffa second order integration happens if one the velocity straight commands (linear or lateral) is 0.

Test it

run mecanum_drive.sdf and listen the odometry and tf topics.

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
Updated documentation (as needed)
Updated migration guide (as needed)
Consider updating Python bindings (if the library has them)
codecheck passed (See contributing)
All tests passed (See test coverage)
While waiting for a review on your PR, please help review another open pull request to support the maintainers

ahcorde

Please check

linters https://github.com/gazebosim/gz-math/runs/8020312206?check_suite_focus=true
Add tests
Sign the commit https://github.com/gazebosim/gz-math/pull/486/checks?check_run_id=8020243561

We are also adding python interfaces to new classes, if you don't have time to do it, please open an issue about adding python interfaces.

include/ignition/math/MecanumDriveOdometry.hh

src/MecanumDriveOdometry.cc

danilogsch · 2022-08-25T16:09:52Z

depends on #1664 of gz-sim

…heels odometry Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com>

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com>

Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com>

ahcorde

linters https://github.com/gazebosim/gz-math/actions/runs/3534393561/jobs/5931205749

  /github/workspace/include/ignition/math/MecanumDriveOdometry.hh:81:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:36:  Line ends in whitespace.  Consider deleting these extra spaces.  [whitespace/end_of_line] [4]
  /github/workspace/src/MecanumDriveOdometry.cc:41:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:144:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:152:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:153:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:154:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:155:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:176:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:177:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:180:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:181:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:182:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:206:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:272:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:285:  Missing username in TODO; it should look like "// TODO(my_username): Stuff."  [readability/todo] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:286:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:287:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  Total errors found: 18

ahcorde

linters https://github.com/gazebosim/gz-math/actions/runs/3583238905/jobs/6034063390

azeey · 2023-02-13T20:21:33Z

@danilogsch Do you think you will have time to fix the linter issues and address @ahcorde's feedback ?

include/ignition/math/MecanumDriveOdometry.hh

mjcarroll · 2023-03-20T18:09:21Z

@danilogsch friendly ping

danilogsch · 2023-03-20T18:49:20Z

hey, sorry that I haven't reached here in a while... Is there anything else you need me to do here?

Bi0T1N · 2023-05-06T14:26:00Z

Looks like linter errors are resolved, only Ubuntu Bionic lists

/github/workspace/include/ignition/math/MecanumDriveOdometry.hh:81:  Line ends in whitespace.  Consider deleting these extra spaces.  [whitespace/end_of_line] [4]
/github/workspace/include/ignition/math/MecanumDriveOdometry.hh:81:  Lines should be <= 80 characters long  [whitespace/line_length] [2]

but this could probably be neglected since Focal/Jammy don't complain.

However, you have to add tests - probably similar to the ones in DiffDriveOdometry_TEST.cc.
Also your last commit isn't signed but one option could be to amend, sign and force-pushing the new commit to your branch.

danilogsch · 2023-05-09T02:45:22Z

@Bi0T1N , thanks for the insight... I had no idea on how to deal with unsigned commits.
Anyway, I have time this week and I will work on the tests.

muttistefano · 2023-05-27T08:19:01Z

Hi, is there anything I can do to help here?
I have been waiting for this to be merged for a while, so if I can help, I would be glad.
Thanks

danilogsch · 2023-06-01T22:29:54Z

Hi, is there anything I can do to help here? I have been waiting for this to be merged for a while, so if I can help, I would be glad. Thanks

Hey, thanks for asking... could you help me with the tests? I've been using Garden in my research, I did not have time to look into the DiffDriveOdometry_TEST.cc and to mimic that just yet

line length corrections

Version v2 of the actions/checkout workflow is deprecated, so switch to v3. Part of gazebo-tooling/release-tools#862. Signed-off-by: Steve Peters <scpeters@openrobotics.org>

The LICENSE file contained a copy of the stanze used at the top of source code files, while the actual license was in the COPYING file. So remove the stanza and put the actual Apache 2.0 license text in LICENSE. Signed-off-by: Steve Peters <scpeters@openrobotics.org>

* Custom PID error rate Signed-off-by: Nate Koenig <natekoenig@gmail.com> * added test Signed-off-by: Nate Koenig <natekoenig@gmail.com> --------- Signed-off-by: Nate Koenig <natekoenig@gmail.com>

Signed-off-by: Michael Carroll <michael@openrobotics.org>

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

muttistefano · 2023-06-02T13:34:37Z

ok,i can take a look into that.
I have a question regarding the pull, I see it targets ign-math6, this means that after that we will have to do another one into the gz-math7 branch ?
Thanks

Bi0T1N · 2023-06-02T19:34:00Z

I have a question regarding the pull, I see it targets ign-math6, this means that after that we will have to do another one into the gz-math7 branch ? Thanks

Yes, as ign-math6 is only for Citadel and Fortress. The merge into gz-math7 should be quite simple as it doesn't touch many other files but it should be rebased on the current HEAD due to the ignition to gz renaming.

danilogsch · 2023-06-06T17:42:16Z

ok,i can take a look into that. I have a question regarding the pull, I see it targets ign-math6, this means that after that we will have to do another one into the gz-math7 branch ? Thanks

I made this two branches when Garden was released:
gz-sim7
gz-math7

basic the same as this PR, feel free to use it

Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai>

Co-authored-by: muttistefano <mutti.stefano.jp@gmail.com> Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai>

mjcarroll · 2023-08-15T21:27:46Z

I just addressed the linting issues here and pulled in @muttistefano's test from #537

azeey

I haven't verified with gz-sim, but I've looked at the equations and the referenced website and it all looks good to me. I'd say the assumption of a 1.0 wheel radius should be explicitly stated in the class header and in the gz-sim system that uses this.

include/gz/math/MecanumDriveOdometry.hh

Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai>

danilogsch requested review from scpeters and adityapande-1995 as code owners August 25, 2022 16:01

github-actions bot added 🏯 fortress Ignition Fortress 🏰 citadel Ignition Citadel labels Aug 25, 2022

danilogsch mentioned this pull request Aug 25, 2022

Add TF publishing in MecanumDrive system gazebosim/gz-sim#1619

Open

ahcorde requested changes Aug 25, 2022

View reviewed changes

danilogsch mentioned this pull request Aug 25, 2022

MecanumDrive odometry and tf implementation gazebosim/gz-sim#1664

Closed

9 tasks

danilogsch and others added 7 commits August 25, 2022 13:56

Class ignition::math::MecanumDriveOdometryPrivate to handle Mecanum w…

54bf59e

…heels odometry Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com>

Update include/ignition/math/MecanumDriveOdometry.hh

c4224f9

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com>

Update include/ignition/math/MecanumDriveOdometry.hh

5e51418

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com>

Update include/ignition/math/MecanumDriveOdometry.hh

9512875

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com>

Update src/MecanumDriveOdometry.cc

6ab64a2

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com>

MecanumDrive doc

113be17

Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com>

MecanumDrive doc

1ac66f5

Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com>

danilogsch force-pushed the ign-math6 branch from f480e47 to 1ac66f5 Compare August 25, 2022 16:57

danilogsch mentioned this pull request Aug 25, 2022

MecanumDrive odometry and tf functions implementation gazebosim/gz-sim#1665

Closed

9 tasks

chapulina added the enhancement New feature or request label Aug 26, 2022

azeey requested a review from ahcorde November 21, 2022 19:25

Merge branch 'ign-math6' into ign-math6

72f8068

ahcorde requested changes Nov 23, 2022

View reviewed changes

ahcorde changed the title ~~Class ignition::math::MecanumDriveOdometryPrivate to handle Mecanum w…~~ MecanumDriveOdometry to handle odometry estimation of Mecanum wheeled models Nov 24, 2022

ahcorde assigned danilogsch Nov 24, 2022

ahcorde requested changes Nov 30, 2022

View reviewed changes

ahcorde requested changes Feb 13, 2023

View reviewed changes

include/ignition/math/MecanumDriveOdometry.hh Outdated Show resolved Hide resolved

danilogsch and others added 8 commits June 1, 2023 20:48

Update MecanumDriveOdometry.cc

320424f

line length corrections

Update include/ignition/math/MecanumDriveOdometry.hh

7e54f8d

CI workflow: use checkout v3 (gazebosim#519)

aa97e31

Version v2 of the actions/checkout workflow is deprecated, so switch to v3. Part of gazebo-tooling/release-tools#862. Signed-off-by: Steve Peters <scpeters@openrobotics.org>

Custom PID error rate (gazebosim#525)

ea6f2fb

* Custom PID error rate Signed-off-by: Nate Koenig <natekoenig@gmail.com> * added test Signed-off-by: Nate Koenig <natekoenig@gmail.com> --------- Signed-off-by: Nate Koenig <natekoenig@gmail.com>

Add option to skip pybind11 and SWIG (gazebosim#480)

8e77c39

Signed-off-by: Michael Carroll <michael@openrobotics.org>

Disable pybind11 on windows by default (gazebosim#529)

e57cc85

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

🎈 6.14.0 (gazebosim#531)

8f2c940

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

danilogsch force-pushed the ign-math6 branch from b4f5fb5 to 8f2c940 Compare June 1, 2023 23:55

Merge branch 'ign-math6' into ign-math6

af31e04

muttistefano mentioned this pull request Jun 20, 2023

MecanumDrive class and test #537

Closed

azeey added the beta Targeting beta release of upcoming collection label Jul 31, 2023

mjcarroll and others added 2 commits August 15, 2023 20:20

Lint

241951b

Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai>

Port changes and tests from muttistefano

b3d4d74

Co-authored-by: muttistefano <mutti.stefano.jp@gmail.com> Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai>

azeey approved these changes Aug 15, 2023

View reviewed changes

include/gz/math/MecanumDriveOdometry.hh Outdated Show resolved Hide resolved

include/gz/math/MecanumDriveOdometry.hh Show resolved Hide resolved

Add accessor and TODO note

6a2af35

Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai>

scpeters mentioned this pull request Aug 21, 2023

Bionic entrypoint.sh: exit 0 gazebo-tooling/action-gz-ci#66

Merged

azeey approved these changes Aug 21, 2023

View reviewed changes

Merge branch 'ign-math6' into ign-math6

a9afe8c

ahcorde approved these changes Aug 22, 2023

View reviewed changes

ahcorde mentioned this pull request Aug 22, 2023

Add MecanumDriveOdometry Python wrapper #548

Open

mjcarroll merged commit 469540c into gazebosim:ign-math6 Aug 22, 2023

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

MecanumDriveOdometry to handle odometry estimation of Mecanum wheeled models #486

MecanumDriveOdometry to handle odometry estimation of Mecanum wheeled models #486

danilogsch commented Aug 25, 2022 •

edited by mjcarroll

Loading

ahcorde left a comment •

edited

Loading

danilogsch commented Aug 25, 2022

ahcorde left a comment

ahcorde left a comment

azeey commented Feb 13, 2023

mjcarroll commented Mar 20, 2023

danilogsch commented Mar 20, 2023

Bi0T1N commented May 6, 2023

danilogsch commented May 9, 2023

muttistefano commented May 27, 2023

danilogsch commented Jun 1, 2023

muttistefano commented Jun 2, 2023

Bi0T1N commented Jun 2, 2023

danilogsch commented Jun 6, 2023

mjcarroll commented Aug 15, 2023

azeey left a comment

MecanumDriveOdometry to handle odometry estimation of Mecanum wheeled models #486

MecanumDriveOdometry to handle odometry estimation of Mecanum wheeled models #486

Conversation

danilogsch commented Aug 25, 2022 • edited by mjcarroll Loading

🎉 New feature

Summary

Test it

Checklist

ahcorde left a comment • edited Loading

Choose a reason for hiding this comment

danilogsch commented Aug 25, 2022

ahcorde left a comment

Choose a reason for hiding this comment

ahcorde left a comment

Choose a reason for hiding this comment

azeey commented Feb 13, 2023

mjcarroll commented Mar 20, 2023

danilogsch commented Mar 20, 2023

Bi0T1N commented May 6, 2023

danilogsch commented May 9, 2023

muttistefano commented May 27, 2023

danilogsch commented Jun 1, 2023

muttistefano commented Jun 2, 2023

Bi0T1N commented Jun 2, 2023

danilogsch commented Jun 6, 2023

mjcarroll commented Aug 15, 2023

azeey left a comment

Choose a reason for hiding this comment

danilogsch commented Aug 25, 2022 •

edited by mjcarroll

Loading

ahcorde left a comment •

edited

Loading